#include "PhysicsSystem.hpp"

using namespace Typhoon;

PhysicsSystem::PhysicsSystem() 
	: Gravity(this)
{
	collisionConfiguration = new btDefaultCollisionConfiguration();
	dispatcher = new btCollisionDispatcher(collisionConfiguration);
	broadphase = new btDbvtBroadphase();
	solver = new btSequentialImpulseConstraintSolver();
	world = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver,collisionConfiguration);
		
	AddComponent<RigidBody>();
	AddComponent<Transform>();
	isSimulating = false;
	joints.world = world;
	joints.physics = this;
	subSystems.push_back(&joints);

	Gravity.Changed += event_handler(this, &PhysicsSystem::GravityChanged);

	Gravity = Vector3(0,-10,0);
}

PhysicsSystem::~PhysicsSystem() {
	delete world;
	delete solver;
	delete broadphase;
	delete dispatcher;
	delete collisionConfiguration;
}

void PhysicsSystem::Update(float dt) {

	world->stepSimulation(dt);

	isSimulating = true;
	for (Bodies::iterator it = bodies.begin(); it!=bodies.end(); ++it) {
		btRigidBody* body = (*it);
		Transform* transform = (Transform*)body->getUserPointer();
		btTransform world;
		body->getMotionState()->getWorldTransform(world);
		btVector3 position = world.getOrigin();
		btQuaternion rotation = world.getRotation();
		transform->Position = Vector3(position.x(),position.y(),position.z());
		transform->Rotation = Quaternion(rotation.w(), rotation.x(), rotation.y(), rotation.z());
	}
	isSimulating = false;
	
}

void PhysicsSystem::GameObjectAdded(GameObject* gameObject) {
	RigidBody* rigidBody = gameObject->GetComponent<RigidBody>();
	if (!rigidBody->Body) return;

	Transform* transform = gameObject->GetComponent<Transform>();

	
	const Matrix4x4& world = *transform->World.GetValue();
	Vector3 position = world.Translation();
	Quaternion rotation;
	rotation.FromRotationMatrix(world);
	btTransform startTransform;
	startTransform.setOrigin(btVector3(position.x,position.y,position.z));
	startTransform.setRotation(btQuaternion(rotation.x, rotation.y, rotation.z, rotation.w));

	btDefaultMotionState* motionState = new btDefaultMotionState(startTransform);
	
	btRigidBody::btRigidBodyConstructionInfo rbInfo(*rigidBody->Body);
	rbInfo.m_motionState = motionState;

	btRigidBody* body = new btRigidBody(rbInfo);
	body->setUserPointer(transform);
	motionStates.push_back(motionState);
	bodies.push_back(body);
	
	AddData(rigidBody, body);

	this->world->addRigidBody(body);

	rigidBody->ImpulseReceived +=event_handler(this, &PhysicsSystem::ImpulseReceived);

	//transform->World.Changed += event_handler(this, &PhysicsSystem::TransformChanged);
}

void PhysicsSystem::GameObjectRemoved(GameObject* gameObject) {
	RigidBody* rigidBody = gameObject->GetComponent<RigidBody>();
	if (!rigidBody->Body) return;
	Transform* transform = gameObject->GetComponent<Transform>();
	btRigidBody* body = (btRigidBody*)GetData(rigidBody);
	bodies.remove(body);
	motionStates.remove(body->getMotionState());
	this->world->removeRigidBody(body);
	delete body->getMotionState();
	delete body;

	rigidBody->ImpulseReceived -=event_handler(this, &PhysicsSystem::ImpulseReceived);
	//transform->Transform.Changed -= event_handler(this, &PhysicsSystem::TransformChanged);
}


void PhysicsSystem::ImpulseReceived(RigidBody::ImpulseEvent& impulse) {
	btRigidBody* body = (btRigidBody*)GetData(impulse.body);
	body->applyImpulse(btVector3(impulse.impulse.x, impulse.impulse.y, impulse.impulse.z),
		btVector3(impulse.localPosition.x, impulse.localPosition.y, impulse.localPosition.z));
}

/*
void PhysicsSystem::TransformChanged( WorldTransform* transform ) {
	if (isSimulating) return;
	RigidBody* rigidBody = transform->gameObject->GetComponent<RigidBody>();
	b2Body* body = (b2Body*)GetData(rigidBody);
	const Matrix3x3& world = transform->Transform.GetValue();
	Vector2 position = world.Position();
	body->SetTransform(b2Vec2(position.x, position.y), world.Angle());
}
*/
PhysicsSystem::JointSystem::JointSystem() {
	//AddComponent<Joint>();
}

void PhysicsSystem::JointSystem::GameObjectAdded(GameObject* gameObject) {
	/*
	Joint* joint = gameObject->GetComponent<Joint>();
	if (!joint->Type) return;
	joint->Type->bodyA = (b2Body*)physics->GetData(joint->BodyA);
	joint->Type->bodyB = (b2Body*)physics->GetData(joint->BodyB);
	b2Joint* physicsJoint = world->CreateJoint(joint->Type);
	AddData(joint, physicsJoint);
	*/
}

void PhysicsSystem::JointSystem::GameObjectRemoved(GameObject* gameObject) {
	/*
	Joint* joint = gameObject->GetComponent<Joint>();
	b2Joint* physicsJoint = (b2Joint*)GetData(joint);
	if (!physicsJoint) return;
	world->DestroyJoint(physicsJoint);
	*/
}

void PhysicsSystem::GravityChanged(PhysicsSystem* physicsSystem) {
	const Vector3& gravity = Gravity.GetValue();
	world->setGravity(btVector3(gravity.x, gravity.y, gravity.z));
}